import rospy
from gazebo_msgs.srv import GetJointProperties
import numpy as np

class dof_state:
    def __init__(self,service_name='/gazebo/get_joint_properties',joint_name_list='') -> None:
        # rospy.init_node('get_joint_properties_client', anonymous=True)
        self.service_name=service_name
        self.joint_name_list=joint_name_list


    def get_joint_properties(self):       
        position_list,rate_list=[],[]
        rospy.wait_for_service(self.service_name)
        try:
            get_joint_prop_service = rospy.ServiceProxy(self.service_name, GetJointProperties)
            for joint_name in self.joint_name_list:
                response = get_joint_prop_service(joint_name)
                if response.success:
                # print("joint_name:  ",joint_name)
                # print("Position: ", response.position)
                # print("Velocity: ", response.rate)  # 或者根据服务实际返回的字段调整
                # print("Damping: ", response.damping)  # 同样，确认服务返回的结构
                # print("Success: ", response.success)
                # print("Status Message: ", response.status_message)
                    position_list.append(response.position)
                    rate_list.append(response.rate)
                else:
                    print("get_joint_properties :{} failed".format(joint_name))
                    position_list.append(0)
                    rate_list.append(0)
            combined_matrix = np.column_stack((np.array(position_list), np.array(rate_list)))
            return combined_matrix
            

        except rospy.ServiceException as e:
            print("Service call failed: %s"%e)
            return np.zeros((12,2),dtype=np.float32)
        

if __name__ == "__main__":
    service_name='/gazebo/get_joint_properties'
    joint_name_list=['FL_hip_joint', 
                        'FL_thigh_joint',
                        'FL_calf_joint', 
                        'FR_hip_joint', 
                        'FR_thigh_joint', 
                        'FR_calf_joint', 
                        'RL_hip_joint', 
                        'RL_thigh_joint', 
                        'RL_calf_joint', 
                        'RR_hip_joint', 
                        'RR_thigh_joint', 
                        'RR_calf_joint']
    geter=dof_state(service_name,joint_name_list)
    geter.get_joint_properties()
